/*
 * @FileName: agv_handler.cpp
 * @Author: weibin.yin
 * @Date: 2023-12-18 09:26:52
 * @Last Modified by: 
 * @Last Modified time: 
 */
#include "manual_handler.h"

void HD1000::changeOpMode_Req(uint8_t mode)
{
  agv_srvs::SetOperationMode srv;

  std::string modeStr;
  switch (mode)
  {
    case agv_msgs::HD1000::AUTO:
      modeStr= SET_MODE_AUTO;
      break;
    case agv_msgs::HD1000::HALFAUTO:
      modeStr= SET_MODE_MANUL;
      break;
    case agv_msgs::HD1000::MANUAL:
      modeStr= SET_MODE_MANUL;
      break;    
    default:
      break;
  }

  srv.request.mode = modeStr;
  srv.request.sender = 0;
  srv.request.submode = "";
  if(m_operationModeClient.call(srv))
  {
    ROS_INFO("Set Operation mode success!");
  }
  else {
    ROS_ERROR("Set Operation mode failed");
  }
}

void HD1000::joy_cb(const std_msgs::UInt16MultiArray::ConstPtr multArr)
{
  agv_msgs::HD1000  temp_state;
  if ( multArr->data.size() != 7 ) {
    ROS_ERROR("[joy_cb] wrong msg size:%lu", multArr->data.size());  
    return;  
  }
  std::lock_guard<std::mutex> locker(mutex_);

  temp_state.valid= true;
  temp_state.angle= multArr->data[0];
  temp_state.speed= multArr->data[1];
  temp_state.buttons= multArr->data[6];
  temp_state.auto_mode= getBit( multArr->data[6], agv_msgs::HD1000::Auto_BIT);
  temp_state.halfAuto_mode= getBit( multArr->data[6], agv_msgs::HD1000::HalfAuto_BIT);
  temp_state.manual_mode= getBit( multArr->data[6], agv_msgs::HD1000::Manual_BIT);
  temp_state.forward= getBit( multArr->data[6], agv_msgs::HD1000::Forward_BIT); 
  temp_state.low_move= getBit( multArr->data[6], agv_msgs::HD1000::Low_Move_BIT);       
  temp_state.high_move= getBit( multArr->data[6], agv_msgs::HD1000::High_Move_BIT);    
  temp_state.reset= getBit( multArr->data[6], agv_msgs::HD1000::Reset_BIT); 
  temp_state.load1_up= getBit( multArr->data[6], agv_msgs::HD1000::Load1_Up_BIT); 
  temp_state.load1_down= getBit( multArr->data[6], agv_msgs::HD1000::Load1_Down_BIT); 
  temp_state.load2_left= getBit( multArr->data[6], agv_msgs::HD1000::Load2_Left_BIT); 
  temp_state.load2_right= getBit( multArr->data[6], agv_msgs::HD1000::Load2_Right_BIT); 
  temp_state.e_stop= getBit( multArr->data[6], agv_msgs::HD1000::EStop_BIT); 
  temp_state.backward= getBit( multArr->data[6], agv_msgs::HD1000::Backward_BIT); 
  temp_state.deadman= getBit( multArr->data[6], agv_msgs::HD1000::Deadman_BIT); 

  if ( temp_state.angle > 15000 || temp_state.angle < -15000 ) {
    ROS_ERROR("[joy_cb] wrong angle value:%d", temp_state.angle);  
    return;  
  }
  if ( temp_state.speed > 100 || temp_state.speed < 0 ) {
    ROS_ERROR("[joy_cb] wrong speed value:%d", temp_state.speed);  
    return;  
  }
  if ( (temp_state.auto_mode && temp_state.halfAuto_mode)
    || (temp_state.auto_mode && temp_state.manual_mode)
    || (temp_state.halfAuto_mode && temp_state.manual_mode)
    || (temp_state.low_move && temp_state.high_move)) {
    ROS_ERROR("[joy_cb] wrong buttons value:%d", temp_state.buttons);  
    return;  
  }

  // nomal HD1000 msg
  if ( temp_state.auto_mode) 
    temp_state.operation_mode= agv_msgs::HD1000::AUTO;
  else if ( temp_state.halfAuto_mode)
    temp_state.operation_mode= agv_msgs::HD1000::HALFAUTO;
  else
    temp_state.operation_mode= agv_msgs::HD1000::MANUAL;
  if ( temp_state.load1_up)
  {
    periph_ctl_button_ = UP;
  }
  else if (temp_state.load1_down)
  {
    periph_ctl_button_ = DOWN;
  }
  else if (temp_state.load2_left)
  {
    periph_ctl_button_ = CLOCKWISE_ROTATION;
  }
  else if (temp_state.load2_right)
  {
    periph_ctl_button_ = ANTICLOCKWISE_ROTAION;
  }  
  else
  {
    periph_ctl_button_ = NO_ACTIVE;
  }

  if (temp_state.low_move || temp_state.high_move)
  {
    ROS_INFO("[joy_cb]move pressed");
  }

  temp_state.resultant_vel= curve_.currentSpeed;
  temp_state.acc= curve_.accRT;
  temp_state.jerk= curve_.jerk;
  manHandler_pub_.publish(temp_state);    
  hand_state_= temp_state;
  new_come_time_HD1000_ = boost::chrono::system_clock::now();
  active_= true;
}

void HD1000::event_Callback(const agv_msgs::AGVEventStatus::ConstPtr event)
{
    event_= *event;
    std_msgs::UInt8MultiArray ledState;
    for (size_t i = 0; i < 1; i++)
    {
      std_msgs::UInt8  btnElement;
      btnElement.data = event->ecode == agv_msgs::AGVEvent::EMG_STOP_EVENT;
      ledState.data.push_back( btnElement.data);
    }
    handlerLED_pub_.publish(ledState);
}

void ManHandler::inputmap_Callback(const agv_msgs::ReadInPutsNew::ConstPtr msg)
{
  static bool chassisMode_last;
  std::vector<agv_msgs::PairTypeInt> inputs_msg = msg->array;
  for (auto &data : inputs_msg){
      if (data.key == "driveManulAuto"){
          inputs_.chassisMode= (data.value == 1);
      }
      else if(data.key == "forwardMove"){
          inputs_.forwardMove= (data.value == 1);
      }
      else if(data.key == "backwardMove"){
          inputs_.backwardMove= (data.value == 1);
      }
      else if(data.key == "leftCrab"){
          inputs_.leftCrab= (data.value == 1);
      }
      else if(data.key == "rightCrab"){
          inputs_.rightCrab= (data.value == 1);
      }      
      else if(data.key == "leftTurn"){
          inputs_.leftTurn= (data.value == 1);
      }      
      else if(data.key == "rightTurn"){
          inputs_.rightTurn= (data.value == 1);
      }      
  }

  if (inputs_.forwardMove)
    target_vel_.linear.x = ioVelocity_;
  else if (inputs_.backwardMove)
    target_vel_.linear.x = -1 * ioVelocity_;
  else
    target_vel_.linear.x = 0.0;

  if (inputs_.leftCrab)
    target_vel_.linear.y = ioVelocity_;
  else if (inputs_.rightCrab)
    target_vel_.linear.y = -1 * ioVelocity_;
  else
    target_vel_.linear.y = 0.0;

  if (inputs_.leftTurn)
    target_vel_.angular.z = ioVelocity_;
  else if (inputs_.rightTurn)
    target_vel_.angular.z = -1 * ioVelocity_;
  else
    target_vel_.angular.z = 0.0;  

  if ( haveVel(target_vel_))
  {
    new_come_time_keyboard_ = boost::chrono::system_clock::now();
    keyboardActive_= true;
    ROS_WARN_STREAM("[inputmap_Callback] keyboardActive_"<< keyboardActive_);
  }

  if (chassisMode_last != inputs_.chassisMode){
    chassisMode_last= inputs_.chassisMode;
    openAutoRelay_Req( inputs_.chassisMode);
    ROS_INFO("[HD1000 inputmap_Callback]chassisMode:%d,\n\n", inputs_.chassisMode);
  }

}


/*!
 * \brief Initializes node to get parameters, subscribe to topics.
 */
void HD1000::init()
{
  joy_sub_ = n_.subscribe("/HD1000/CMD0001",1,&ManHandler::joy_cb,this);
  agvmode_sub_ = n_.subscribe("/agvmode",1,&ManHandler::agvmodeCallback,this);
  eventOccurSub_ = n_.subscribe("/eventstatus", 1, &ManHandler::event_Callback,this); 
  if (driveControl_ >= 0) handlerEnableSub_ = n_.subscribe("/readInputNew", 1, &ManHandler::inputmap_Callback,this);
  m_outputMapClient = n_.serviceClient<agv_srvs::OutputsNew>("/outputNew");
  manHandler_pub_ = n_.advertise<agv_msgs::HD1000>("/manualDevice", 1);
  handlerLED_pub_ = n_.advertise<std_msgs::UInt8MultiArray>("/HD1000/LEDState", 1);
}